﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Runtime.Intrinsics;
using System.Runtime.Intrinsics.X86;
using System.Text;
using System.Threading.Tasks;
using static System.Runtime.InteropServices.JavaScript.JSType;

namespace SimdTest2.Methods
{
    [Algorithm(true, AlgorithmType.Six)]
    public unsafe class SixMethod : AlgorithmBase
    {
        public SixMethod(int row, int col) : base(row, col)
        {
        }
        Point[] _points;
        int[] xArray;
        int[] yArray;
        protected override void CreateData()
        {
            _points= new Point[_size];
            xArray= new int[_size];
            yArray= new int[_size];
            Random random = new Random();
            for (int i = 0; i < _size; i++)
            {
                int x = random.Next(10, 100);
                int y = random.Next(10, 100);
                _points[i] = new Point(x, y);
                xArray[i]= x;
                yArray[i]= y;
            }
        }
        protected override void OrdinaryMethod()
        {
            fitCircle(_points, out var p, out var r);
            //Trace.WriteLine($"OrdinaryMethod：x={p.X},y={p.Y},r={r}");
        }
        protected override void SimdMethod()
        {
            fitCircleSimd(_points, out var p, out var r);
            //Trace.WriteLine($"SimdMethod：x={p.X},y={p.Y},r={r}");
        }
        private void fitCircleSimd(Point[] pointArray, out PointF center, out double radius)
        {
            int N = pointArray.Length;
            center = new Point();
            radius = 0d;
            if (N < 3)
                return;

            int vsize = Vector128<int>.Count;
            Vector128<int> sumXV = new Vector128<int>();
            Vector128<int> sumYV = new Vector128<int>();
            Vector128<int> sumX2V = new Vector128<int>();
            Vector128<int> sumY2V = new Vector128<int>();
            Vector128<int> sumX3V= new Vector128<int>();
            Vector128<int> sumY3V = new Vector128<int>();
            Vector128<int> sumXYV = new Vector128<int>();
            Vector128<int> sumXY2V = new Vector128<int>();
            Vector128<int> sumX2YV = new Vector128<int>();
            fixed (int* xptr = xArray, yptr = yArray)
            {
                for (int pId = 0; pId < N; pId += vsize)
                {
                    Vector128<int> xv = *(Vector128<int>*)(xptr + pId);
                    Vector128<int> yv = *(Vector128<int>*)(yptr + pId);
                    sumXV = Sse2.Add(sumXV, xv);
                    sumYV = Sse2.Add(sumYV, yv);
                    Vector128<int> x2v = Sse41.MultiplyLow(xv, xv);
                    Vector128<int> y2v = Sse41.MultiplyLow(yv, yv);
                    sumX2V = Sse2.Add(sumX2V, x2v);
                    sumY2V = Sse2.Add(sumY2V, y2v);
                    sumX3V = Sse2.Add(sumX3V, Sse41.MultiplyLow(x2v, xv));
                    sumY3V = Sse2.Add(sumY3V, Sse41.MultiplyLow(y2v, yv));
                    sumXYV = Sse2.Add(sumXYV, Sse41.MultiplyLow(xv, yv));
                    sumXY2V = Sse2.Add(sumXY2V, Sse41.MultiplyLow(xv, y2v));
                    sumX2YV = Sse2.Add(sumX2YV, Sse41.MultiplyLow(x2v, yv));
                }
            }
            int* sumXP = (int*)&sumXV;
            int* sumYP= (int*)&sumYV;
            int* sumX2P = (int*)&sumX2V;
            int* sumY2P = (int*)&sumY2V;
            int* sumX3P = (int*)&sumX3V;
            int* sumY3P = (int*)&sumY3V;
            int* sumXYP = (int*)&sumXYV;
            int* sumXY2P = (int*)&sumXY2V;
            int* sumX2YP = (int*)&sumX2YV;
            double sumX = 0.0d;
            double sumY = 0.0d;
            double sumX2 = 0.0d;
            double sumY2 = 0.0d;
            double sumX3 = 0.0d;
            double sumY3 = 0.0d;
            double sumXY = 0.0d;
            double sumXY2 = 0.0d;
            double sumX2Y = 0.0d;
            for (int i = 0; i < vsize; i++)
            {
                sumX += *(sumXP + i);
                sumY += *(sumYP + i);
                sumX2 += *(sumX2P + i);
                sumY2 += *(sumY2P + i);
                sumX3 += *(sumX3P + i);
                sumY3 += *(sumY3P + i);
                sumXY += *(sumXYP + i);
                sumXY2 += *(sumXY2P + i);
                sumX2Y += *(sumX2YP + i);
            }
            double C = 0d, D = 0d, E = 0d, G = 0d, H = 0d;
            double a = 0d, b = 0d, c = 0d;
            C = N * sumX2 - sumX * sumX;
            D = N * sumXY - sumX * sumY;
            E = N * sumX3 + N * sumXY2 - (sumX2 + sumY2) * sumX;
            G = N * sumY2 - sumY * sumY;
            H = N * sumX2Y + N * sumY3 - (sumX2 + sumY2) * sumY;

            a = (H * D - E * G) / (C * G - D * D);
            b = (H * C - E * D) / (D * D - G * C);
            c = -(a * sumX + b * sumY + sumX2 + sumY2) / N;

            center.X = (float)(-a / 2.0);
            center.Y = (float)(-b / 2.0);
            radius = Math.Sqrt(a * a + b * b - 4 * c) / 2.0;
        }
        private void fitCircle(Point[] pointArray, out PointF center, out double radius)
        {
            int N = pointArray.Length;
            center = new PointF();
            radius = 0d;
            if (N < 3)
            {
                return;
            }
            double sumX = 0.0d;
            double sumY = 0.0d;
            double sumX2 = 0.0d;
            double sumY2 = 0.0d;
            double sumX3 = 0.0d;
            double sumY3 = 0.0d;
            double sumXY = 0.0d;
            double sumXY2 = 0.0d;
            double sumX2Y = 0.0d;
            for (int pId = 0; pId < N; ++pId)
            {
                sumX += pointArray[pId].X;
                sumY += pointArray[pId].Y;
                double x2 = pointArray[pId].X * pointArray[pId].X;
                double y2 = pointArray[pId].Y * pointArray[pId].Y;
                sumX2 += x2;
                sumY2 += y2;

                sumX3 += x2 * pointArray[pId].X;
                sumY3 += y2 * pointArray[pId].Y;
                sumXY += pointArray[pId].X * pointArray[pId].Y;
                sumXY2 += pointArray[pId].X * y2;
                sumX2Y += x2 * pointArray[pId].Y;
            }
            double C = 0d, D = 0d, E = 0d, G = 0d, H = 0d;
            double a = 0d, b = 0d, c = 0d;
            C = N * sumX2 - sumX * sumX;
            D = N * sumXY - sumX * sumY;
            E = N * sumX3 + N * sumXY2 - (sumX2 + sumY2) * sumX;
            G = N * sumY2 - sumY * sumY;
            H = N * sumX2Y + N * sumY3 - (sumX2 + sumY2) * sumY;

            a = (H * D - E * G) / (C * G - D * D);
            b = (H * C - E * D) / (D * D - G * C);
            c = -(a * sumX + b * sumY + sumX2 + sumY2) / N;

            center.X = (float)(-a / 2.0);
            center.Y = (float)(-b / 2.0);
            radius = Math.Sqrt(a * a + b * b - 4 * c) / 2.0;
        }

    }
}
